
#ifndef QUATERNION_H
#define QUATERNION_H

#include "Vector3.h"
#include "RotationMatrix.h"

/*
 * Formal Quaternion Defintion.
 * Notational Form: q = xi + yj + zk + w
 */
template <typename Real>
class Quaternion {
	enum Axis { X, Y, Z, W, COMPONENT_COUNT };

public:
	Quaternion();
	Quaternion(const Quaternion<Real>& q);
	Quaternion(const Vector3<Real>& axis, Real angle);
	Quaternion(Real x, Real y, Real z, Real angle);
	virtual ~Quaternion();

	RotationMatrix<Real> toRotationMatrix() const;

	Quaternion<Real>& operator = (const Quaternion<Real>& q);

	static Real Length(const Quaternion<Real>& q);

	static Quaternion<Real> FromAxisAngle(Real x, Real y, Real z, Real angle);
	static Quaternion<Real> Normalize(const Quaternion<Real>& q);
	static Quaternion<Real> Conjugate(const Quaternion<Real>& q);
	static Quaternion<Real> Mulitiply(const Quaternion<Real>& p, const Quaternion<Real>& q);
	static Quaternion<Real> Identity();

	static void LoadIdentity(Quaternion<Real>& q);
	static void LoadFromAxisAngle(Quaternion<Real>& q, Real x, Real y, Real z, Real angle);
	static Real InnerProduct(const Quaternion<Real>& p, const Quaternion<Real>& q);
	static RotationMatrix<Real> ToRotationMatrix(const Quaternion<Real>& q);

protected:
	Real data[COMPONENT_COUNT];
};

template<typename Real>
inline Quaternion<Real>::Quaternion() {
	Quaternion<Real>::LoadIdentity(*this);
}

template<typename Real>
inline Quaternion<Real>::Quaternion(const Quaternion<Real>& q) {
	this->data[X] = q.data[X];
	this->data[Y] = q.data[Y];
	this->data[Z] = q.data[Z];
	this->data[W] = q.data[W];
}

template<typename Real>
inline Quaternion<Real>::Quaternion(const Vector3<Real>& axis, Real angle) {
	Quaternion<Real>::LoadFromAxisAngle(*this, axis.x(), axis.y(), axis.z(), angle);
}

template<typename Real>
inline Quaternion<Real>::Quaternion(Real x, Real y, Real z, Real angle) {
	Quaternion<Real>::LoadFromAxisAngle(*this, x, y, z);
}

template<typename Real>
inline Quaternion<Real>::~Quaternion() {}

template<typename Real>
inline RotationMatrix<Real> Quaternion<Real>::toRotationMatrix() const {
	return Quaternion<Real>::ToRotationMatrix(*this);
}

template<typename Real>
inline Quaternion<Real>& Quaternion<Real>::operator = (const Quaternion<Real>& q) {
	if ( *this == q ) return *this;
	this->data[X] = q.data[X];
	this->data[Y] = q.data[Y];
	this->data[Z] = q.data[Z];
	this->data[W] = q.data[W];
	return *this;
}

template<typename Real>
inline Real Quaternion<Real>::Length(const Quaternion<Real>& q) {
	Real length = std::sqrt(
		std::pow(q.data[X], static_cast<Real>(2)) + 
		std::pow(q.data[Y], static_cast<Real>(2)) + 
		std::pow(q.data[Z], static_cast<Real>(2)) +
		std::pow(q.data[W], static_cast<Real>(2))
		);
	return length;
}

template<typename Real>
inline Quaternion<Real> Quaternion<Real>::FromAxisAngle(Real x, Real y, Real z, Real angle) {
	Quaternion<Real> q;
	Quaternion<Real>::LoadFromAxisAngle(q, x, y, z);
	return q;
}

template<typename Real>
inline Quaternion<Real> Quaternion<Real>::Normalize(const Quaternion<Real>& q) {
	Quaternion<Real> result;
	Real invLen = static_cast<Real>(1) / Length(q);
	result.data[X] = q.data[X] * invLen;
	result.data[Y] = q.data[Y] * invLen;
	result.data[Z] = q.data[Z] * invLen;
	result.data[W] = q.data[W] * invLen;
	return result;
}

template<typename Real>
inline Quaternion<Real> Quaternion<Real>::Conjugate(const Quaternion<Real>& q) {
	Quaternion<Real> result;
	result.data[X] = -q.data[X];
	result.data[Y] = -q.data[Y];
	result.data[Z] = -q.data[Z];
	result.data[W] = q.data[W];
	return result;
}

template<typename Real>
inline Quaternion<Real> Quaternion<Real>::Mulitiply(const Quaternion<Real>& p, const Quaternion<Real>& q) {
	Quaternion<Real> result;
	result.data[X] = p.data[X] * q.data[X] - p.data[Y] * q.data[Y] - p.data[Z] * q.data[Z] - p.data[W] * q.data[W];
	result.data[Y] = p.data[Y] * q.data[X] + p.data[X] * q.data[Y] + p.data[Z] * q.data[W] - p.data[W] * q.data[Z];
	result.data[Z] = p.data[Z] * q.data[X] + p.data[X] * q.data[Z] + p.data[W] * q.data[Y] - p.data[Y] * q.data[W];
	result.data[W] = p.data[W] * q.data[X] + p.data[X] * q.data[W] + p.data[Y] * q.data[Z] - p.data[Z] * q.data[Y];
	return result;
}

template<typename Real>
inline Quaternion<Real> Quaternion<Real>::Identity() {
	Quaternion<Real> i;
	i.data[X] = static_cast<Real>(0);
	i.data[Y] = static_cast<Real>(0);
	i.data[Z] = static_cast<Real>(0);
	i.data[W] = static_cast<Real>(1);
	return i;
}

template<typename Real>
inline void Quaternion<Real>::LoadIdentity(Quaternion<Real>& q) {
	q.data[X] = static_cast<Real>(0);
	q.data[Y] = static_cast<Real>(0);
	q.data[Z] = static_cast<Real>(0);
	q.data[W] = static_cast<Real>(1);
}

template<typename Real>
inline void Quaternion<Real>::LoadFromAxisAngle(Quaternion<Real>& q, Real x, Real y, Real z, Real angle) {
	Real halfAngle = angle * static_cast<Real>(0.5);
	Real sinHalfAngle = std::sin(halfAngle);
	q.data[W] = std::cos(angle * static_cast<Real>(0.5));
	q.data[X] = x * sinHalfAngle;
	q.data[Y] = y * sinHalfAngle;
	q.data[Z] = z * sinHalfAngle;
}

template<typename Real>
inline Real Quaternion<Real>::InnerProduct(const Quaternion<Real>& p, const Quaternion<Real>& q) {
	return p.data[X] * q.data[X] + p.data[Y] * q.data[Y] + p.data[Z] * q.data[Z] + p.data[W] * q.data[W];
}

template<typename Real>
inline RotationMatrix<Real> Quaternion<Real>::ToRotationMatrix(const Quaternion<Real>& q) {
	Quaternion<Real> qn = Quaternion<Real>::Normalize(q);
	RotationMatrix<Real> rot;

	Real o = static_cast<Real>(1);
	Real t = static_cast<Real>(2);
	Real x = qn.data[X];
	Real y = qn.data[Y];
	Real z = qn.data[Z];
	Real w = qn.data[W];
	Real xs = x * x;
	Real ys = y * y;
	Real zs = z * z;

	rot[Matrix3<Real>::A_11] = o - t * ys - t * zs;
	rot[Matrix3<Real>::A_22] = o - t * xs - t * zs;
	rot[Matrix3<Real>::A_33] = o - t * xs - t * ys;

	rot[Matrix3<Real>::A_12] = rot[Matrix3<Real>::A_21] = t * x * y - t * w * z;
	rot[Matrix3<Real>::A_13] = rot[Matrix3<Real>::A_31] = t * x * z + t * w * y;
	rot[Matrix3<Real>::A_23] = rot[Matrix3<Real>::A_32] = t * y * z + t * w * x;
	return rot;
}

typedef Quaternion<float> Quaternionf;
typedef Quaternion<double> Quaterniond;

#endif
